#include "ArmorPlate.h"
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

ArmorPlate::ArmorPlate()
{
    myteam = REDTEAM;
}

bool ArmorPlate::CameraInit(int device)
{
    capture_plate.open(device);
    if (!capture_plate.isOpened())
    {
        cout << "The capture has something wrong!";
        return false;
    }
    else return true;
}

cv::RotatedRect& adjustRec(cv::RotatedRect& rec)
{
    using std::swap;

    float& width = rec.size.width;
    float& height = rec.size.height;
    float& angle = rec.angle;

/*
    while (angle >= 90.0) angle -= 180.0;
    while (angle < -90.0) angle += 180.0;
*/


    if (angle >= 45.0)
    {
        swap(width, height);
        angle -= 90.0;
    }
    else if (angle < -45.0)
    {
        swap(width, height);
        angle += 90.0;
    }


    return rec;
}

void ArmorPlate::AutoShoot()
{
    //armor_image = imread("/home/rm/opencv_test/Hero.jpg");
    Mat raw_image = imread("/home/rm/ArmorDetector/test03.jpg");
    //Mat raw_image = imread("/home/rm/ArmorDetector/test04.png");
    armor_image=raw_image;
    namedWindow("raw_img", WINDOW_NORMAL);
    imshow("raw_img", armor_image);
    ImgPreprosses(armor_image, pre_image);
    waitKey(0);
}

void ArmorPlate::ImportVideo()
{
    //VideoCapture capture("/home/rm/ArmorDetector/步兵素材蓝车侧面-ev-0.MOV");
//      VideoCapture capture("/home/rm/ArmorDetector/步兵素材蓝车旋转-ev--3.MOV");
//    VideoCapture capture("/home/rm/ArmorDetector/algorithm_testing.mp4");
//    VideoCapture capture("/home/rm/ArmorDetector/反陀螺慢动作效果.mp4");
//    VideoCapture capture("/home/rm/ArmorDetector/红色1号01.avi");
//    VideoCapture capture(0);
    capture.set(CAP_PROP_EXPOSURE,-10);


    while (1)
    {
        double t = (double)cv::getTickCount();//开始计时

        Mat raw_frame;
        capture >> raw_frame;
        if (raw_frame.empty())
        {
            cout << "Error: Couldn't load the image!" << endl;
            break;
        }
        armor_image = raw_frame;

        namedWindow("raw", WINDOW_NORMAL);
        imshow("raw", armor_image);
        ImgPreprosses(armor_image, pre_image);

        t = ((double)cv::getTickCount() - t) / cv::getTickFrequency();//结束计时
        double fps = int(1.0 / t);//转换为帧率
        cout << "FPS: " << fps<<endl;//输出帧率
        // double fps = capture.get(CAP_PROP_FPS); //获取帧率
        //将帧率显示在图片上
        char str[20];
        string FPSstring("FPS:");
        sprintf(str, "%.2f", fps);
//        cout<<"FPS: "<<fps<<endl;
        FPSstring += str;
        putText(raw_frame, FPSstring,Point(5, 20),
                FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0));

        waitKey(30);

    }
}


void drawall(vector<RotatedRect> rec,Mat img)
{
    for (int i = 0; i < rec.size(); i++)
    {
        Point2f p[4];
        rec[i].points(p);

        double _pm_x=0.5*(p[0].x+p[1].x);
        double _pm_y=0.5*(p[0].y+p[1].y);
        p[0].x=2.34*p[0].x-1.33*_pm_x;
        p[0].y=2.34*p[0].y-1.33*_pm_y;
        p[1].x=2.34*p[1].x-1.33*_pm_x;
        p[1].y=2.34*p[1].y-1.33*_pm_y;

        double _pn_x=0.5*(p[2].x+p[3].x);
        double _pn_y=0.5*(p[2].y+p[3].y);
        p[2].x=2.34*p[2].x-1.33*_pn_x;
        p[2].y=2.34*p[2].y-1.33*_pn_y;
        p[3].x=2.34*p[3].x-1.33*_pn_x;
        p[3].y=2.34*p[3].y-1.33*_pn_y;


        for (int j = 0; j < 4; j++)
        {
//            cout<<p[j]<<endl;
            line(img, p[j], p[(j + 1) % 4], Scalar(0,0,255),2,8,0);
        }

    }
}


void ArmorPlate::ImgPreprosses(const Mat& src, const Mat& dst)
{
    Mat grayImg;
    Mat binBrightImg;
    vector<RotatedRect> lightInfos;
    double MaxValue;
    vector<Mat> channels;
    Mat bin_img;

    //方法2//![](../../学习文件/inRange函数提取颜色范围.png)
    Mat HSVImg;
    Mat image;
    cvtColor(src, HSVImg, COLOR_BGR2HSV);
    split(HSVImg, channels);
    minMaxLoc(channels[2], 0, &MaxValue, 0, 0);
    threshold(channels[2], channels[2], MaxValue*0.98, 255, THRESH_BINARY);
    Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
    medianBlur(channels[2], channels[2], 3);
    morphologyEx(channels[2], channels[2], MORPH_DILATE, element, Point(-1, -1), 1);
    //namedWindow("V通道二值图", WINDOW_NORMAL);
    //imshow("V通道二值图", channels[2]);waitKey(0);

    HSVImg.copyTo(image, channels[2]);
    int BLowH = 80;
    int BHighH = 150;
    int BLowS = 60;
    int BHighS = 255;
    int BLowV = 100;
    int BHighV = 255;
    inRange(image, Scalar(BLowH, BLowS, BLowV), Scalar(BHighH, BHighS, BHighV),bin_img);

/*
    HSVImg.copyTo(image, channels[2]);
    int RLowH = 0;
    int RHighH = 80;
    int RLowS = 10;
    int RHighS = 255;
    int RLowV = 60;
    int RHighV = 255;
    inRange(image, Scalar(RLowH, RLowS, RLowV), Scalar(RHighH, RHighS,RHighV), bin_img);
    namedWindow("bin_img", WINDOW_NORMAL);
    imshow("bin_img", bin_img);
    //binBrightImg = channels[2];*/
    // dilate
    Mat dil_img;
    //Mat element = getStructuringElement(MORPH_RECT, Size(8, 8));
    dilate(bin_img,dil_img, element);
    //namedWindow("dil_img", WINDOW_NORMAL);
    //imshow("dil_img",dil_img);
    // erode
    Mat structureElement = getStructuringElement(MORPH_RECT, Size(8, 8), Point(-1, -1));      //创建结构元
    erode(dil_img,binBrightImg, structureElement);
    //namedWindow("ero_img", WINDOW_NORMAL);
    //imshow("ero_img",binBrightImg);

    //方法2结束//

    vector<vector<Point>> lightContours;
    cv::findContours(binBrightImg.clone(), lightContours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
    Mat drawingImg = Mat::zeros(src.size(), CV_8UC3);
    for (int i = 0; i < lightContours.size(); i++)
        drawContours(drawingImg, lightContours, i, Scalar(0, 255, 0));
    namedWindow("contours1", WINDOW_NORMAL);
    imshow("contours1", drawingImg);

    lightInfos.clear();

    Mat drawingImg2 = Mat::zeros(src.size(), CV_8UC3);
    for (const auto& contour : lightContours)
    {
        float lightContourArea = contourArea(contour);
        if (contour.size() <= 5 ||lightContourArea <10 ) continue;

        RotatedRect lightRec = fitEllipse(contour);
        RotatedRect minAreaRec = minAreaRect(contour);

        if (((minAreaRec.size.height/minAreaRec.size.width)>1 &&
        (minAreaRec.size.height/minAreaRec.size.width)<10 &&minAreaRec.angle>-15
        /*&& minAreaRec.size.area()>50)*/|| ((minAreaRec.size.height/minAreaRec.size.width)<1
        && (minAreaRec.size.height/minAreaRec.size.width)>0.1 &&minAreaRec.angle<-75 /*&& minAreaRec.size.area()>50*/)))
        {
//            cout << "width=" << minAreaRec.size.width << " height=" << minAreaRec.size.height
//                 << " radio=" << minAreaRec.size.height / minAreaRec.size.width << endl;
//            cout << "angle:" << minAreaRec.angle << "  area=" << minAreaRec.size.area() << endl << endl;
            lightInfos.push_back(minAreaRec);
            Point2f vertices[4];
            minAreaRec.points(vertices);
            for (int j = 0; j < 4; j++)
                //line(drawingImg2, vertices[j], vertices[(j + 1) % 4], Scalar(0,255,0),1);
                line(armor_image, vertices[j], vertices[(j + 1) % 4], Scalar(0,255,0),1);
        }


        adjustRec(lightRec);
        if ((lightRec.size.width / lightRec.size.height) >0.8)
            continue;
        int x = lightRec.center.x - lightRec.size.width;
        if (x < 0)
            continue;
        int y = lightRec.center.y - lightRec.size.height;
        if (y < 0)
            continue;
    }
    namedWindow("contours", WINDOW_NORMAL);
    imshow("contours", armor_image);

    vector<RotatedRect> armors;
    vector<ArmorRect> armorRects;
    ArmorRect armorRect;

    armors.clear();
    armorRects.clear();

    if (lightInfos.size()<=1)
    {
        cout << "There's no light contours in quality." << endl;
    }

//    cout<<"number of light="<<lightInfos.size()<<endl;
    sort(lightInfos.begin(), lightInfos.end(), [](const RotatedRect& ld1, const RotatedRect& ld2)
    {
        return ld1.center.x < ld2.center.x;
    });

    for (int i = 0; i < lightInfos.size(); i++)
    {
        for (int j = i + 1; j < lightInfos.size(); j++)
        {
            const RotatedRect& left = lightInfos[i];
            const RotatedRect& right = lightInfos[j];

            double heightDiff = abs(left.size.height - right.size.height);
            double widthDiff = abs(left.size.width - right.size.width);
            double angleDiff = abs(left.angle - right.angle);
            double yDiff = abs(left.center.y - right.center.y);
            double xDiff = abs(left.center.x - right.center.x);
            double mean_height = (left.size.height + right.size.height)/2;
            double mean_width = (left.size.width + right.size.width)/2;
            double yDiffRatio = yDiff / mean_height;
            double xDiffRatio = xDiff / mean_width;

            double dis= sqrt((left.center.x - right.center.x)*(left.center.x - right.center.x) + (left.center.y - right.center.y)*(left.center.y - right.center.y));
            double ratio = dis / mean_height;
            float heightDiff_ratio = heightDiff / max(left.size.height, right.size.height);

//            cout<<"angle_diff="<<angleDiff<<endl;
//            cout<<"xDiffRatio="<<xDiffRatio<<endl;
//            cout<<"yDiffRatio="<<yDiffRatio<<endl;
//            cout<<"ratio="<<ratio<<endl;
            if (angleDiff > 10 || xDiffRatio < 0.5 || yDiffRatio>0.7 ||ratio>5||ratio<1)
                continue;

            armorRect.armors.center.x = (left.center.x + right.center.x) / 2;
            armorRect.armors.center.y = (left.center.y + right.center.y) / 2;
            armorRect.armors.angle= (left.angle + right.angle) / 2;

            //cout <<"(debug3)left.angle="<<left.angle << endl;
            //armorRect.armors.angle = 0;

            if (180 - angleDiff < 3)
                armorRect.armors.angle += 90;
            armorRect.armors.size.height= (left.size.height + right.size.height) / 2;
            armorRect.armors.size.width = sqrt((left.center.x - right.center.x)*(left.center.x - right.center.x) + (left.center.y - right.center.y)*(left.center.y - right.center.y));

            double nL = armorRect.armors.size.height;
            double nW = armorRect.armors.size.width;
            if (nL < nW)
            {
                armorRect.armors.size.height = nL;
                armorRect.armors.size.width = nW;
            }
            else
            {
                armorRect.armors.size.height = nW;
                armorRect.armors.size.width = nL;
            }

            armorRects.emplace_back(armorRect);
            armors.push_back(armorRect.armors);
        }
    }
    if (armorRects.empty())
    //if(armors.empty())
//        cout << "There is no armor in quality!" << endl;

    cout<<"number of armors="<<armors.size()<<endl<<endl;
    drawall(armors, src);
    namedWindow("result", WINDOW_NORMAL);
    imshow("result", src);
}
